#include <functional>
#include <memory>
#include <sstream>
#include <string>
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_ros/transform_broadcaster.h"
#include "nav_msgs/msg/odometry.hpp"
#include <tf2/utils.h>


class FramePublisher : public rclcpp::Node
{
public:
  FramePublisher()
  : Node("originbot_tf2_frame_publisher")
  {
    // Declare and acquire `turtlename` parameter
    turtlename_ = this->declare_parameter<std::string>("robotname", "robot");
    
    this->declare_parameter<std::string>("odom_topic", "/odom");
    this->get_parameter("odom_topic", odom_topic);
    
    // Initialize the transform broadcaster
    tf_broadcaster_ =
      std::make_unique<tf2_ros::TransformBroadcaster>(*this);

    // Subscribe to the /odom topic and call handle_odom_message
    // callback function on each message
    subscription_ = this->create_subscription<nav_msgs::msg::Odometry>(
      odom_topic , 10,
      std::bind(&FramePublisher::handle_odom_message, this, std::placeholders::_1));
      
  }

private:
  void handle_odom_message(const nav_msgs::msg::Odometry::SharedPtr msg)
  {
    geometry_msgs::msg::TransformStamped t;

    // Read message content and assign it to
    // corresponding tf variables
    t.header.stamp = this->get_clock()->now();
    t.header.frame_id = "world";
    t.child_frame_id = turtlename_;

    // Extract x and y coordinates from the Odometry message
    t.transform.translation.x = msg->pose.pose.position.x;
    t.transform.translation.y = msg->pose.pose.position.y;
    t.transform.translation.z = 0.0;


    // Extract the quaternion from the message
    tf2::Quaternion q(
      msg->pose.pose.orientation.x,
      msg->pose.pose.orientation.y,
      msg->pose.pose.orientation.z,
      msg->pose.pose.orientation.w
    );

    // Calculate Yaw angle from the quaternion
    double yaw = tf2::getYaw(q);

    // Set rotation in x and y to 0 and assign the Yaw angle
    t.transform.rotation.x = 0.0;
    t.transform.rotation.y = 0.0;
    t.transform.rotation.z = sin(yaw / 2.0);
    t.transform.rotation.w = cos(yaw / 2.0);
       
    // Send the transformation
    tf_broadcaster_->sendTransform(t);
  }

  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr subscription_;
  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
  std::string turtlename_;
  std::string odom_topic;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<FramePublisher>());
  rclcpp::shutdown();
  return 0;
}

